/*
 * GestureParameterization.h
 *
 *  Created on: Aug 18, 2009
 *      Author: damaraju
 */

#ifndef GESTUREPARAMETERIZATION_H_
#define GESTUREPARAMETERIZATION_H_

#include <touch/Touch.h>

#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/ptr_container/ptr_map.hpp>
#include <boost/ptr_container/serialize_ptr_vector.hpp>

//FIXME Check input strings before proceeding. Provide helpful error messages.
class gesture_parameter
{
public:
	bool		delta;
	vector<int> fingerIndeces;
	unsigned int minFingers;
	set<int> numFingers;
	/**
	 * Dummy operator
	 */
	virtual vector<double> operator()(ContactSetFrame & contactFrame)
	{
		return vector<double>();
	}

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & fingerIndeces;
        ar & minFingers;
        ar & numFingers;
    }
};

class fing_x_parameter : public gesture_parameter
{
public:
	fing_x_parameter(set<int> _numFingers, unsigned int _minFingers, bool _delta, int fing_index)
	{
		delta = _delta;
		numFingers = _numFingers;
		minFingers = _minFingers;
		fingerIndeces.push_back(fing_index);
	}
	virtual vector<double> operator()(ContactSetFrame & contactFrame)
	{
		vector<double> result;
		const vector<Contact> &frame  = contactFrame.frame;
		unsigned int fing_index = fingerIndeces[0];
		if(frame.size() < minFingers || (numFingers.size() > 0 && numFingers.find(frame.size()) == numFingers.end())) //Diff error message if any.
			return result;


		if(frame.size() <= fing_index)
		{
			cout << "Invalid frame for fing_x with index " << fing_index << endl;
			return result;
		}
		result.push_back(frame[fing_index].x);
		return result;
	}
};

class fing_y_parameter : public gesture_parameter
{
public:
	fing_y_parameter(set<int> _numFingers, unsigned int _minFingers, bool _delta, int fing_index)
	{
		delta = _delta;
		numFingers = _numFingers;
		minFingers = _minFingers;
		fingerIndeces.push_back(fing_index);
	}
	virtual vector<double> operator()(ContactSetFrame & contactFrame)
	{
		vector<double> result;
		const vector<Contact> &frame  = contactFrame.frame;
		unsigned int fing_index = fingerIndeces[0];
		if(frame.size() < minFingers || (numFingers.size() > 0 && numFingers.find(frame.size()) == numFingers.end())) //Diff error message if any.
		{
			return result;
		}
		if(frame.size() < fing_index)
		{
			cout << "Invalid frame for fing_y with index " << fing_index << endl;
			return result;
		}
		result.push_back(frame[fing_index].y);
		return result;
	}
};
/**
 * the () operator accepts a contactSetFrame and produces a distance measure.
 * This calculation is prone to error due to change in aspect ratio from 1x1 from CCV-coord space
 * to the actual screen aspect ratio (4:3, 16:9, 16:10, or custom).
 * Use fing_diffXY_parameter to send the difference in x, y b/w two points as results of the param
 */
class fing_dist_parameter : public gesture_parameter
{
public:
	fing_dist_parameter(set<int> _numFingers, unsigned int _minFingers, bool _delta, int finger_index1, int finger_index2)
	{
		delta = _delta;
		numFingers = _numFingers;
		minFingers = _minFingers;
		fingerIndeces.push_back(finger_index1);
		fingerIndeces.push_back(finger_index2);
	}

	virtual vector<double> operator()(ContactSetFrame & contactFrame)
	{
		vector<double> result;
		const vector<Contact> &frame = contactFrame.frame;

		if(frame.size() < minFingers || (numFingers.size() > 0 && numFingers.find(frame.size()) == numFingers.end())) //Diff error message if any.
		{
			return result;
		}
		if(frame.size() < 2) // atleast two fingers required for this test
			return result;
		double dx = (frame[0].x - frame[1].x);
		double dy = (frame[0].y - frame[1].y);
		double dist = sqrt(dx*dx + dy*dy);
		result.push_back(dist);
		return result;
	}
};

/**
* Send the difference in x, y b/w two points as results of the param.
* This is an accurate means of finding pixel distance b/w two contacts by the multitouch application, since it deals with
* screen coord spaces in pixels.
*/
class fing_diffXY_parameter : public gesture_parameter
{
public:
	fing_diffXY_parameter(set<int> _numFingers, unsigned int _minFingers, bool _delta, int finger_index1, int finger_index2)
	{
		delta = _delta;
		numFingers = _numFingers;
		minFingers = _minFingers;
		fingerIndeces.push_back(finger_index1);
		fingerIndeces.push_back(finger_index2);
	}

	virtual vector<double> operator()(ContactSetFrame & contactFrame)
	{
		vector<double> result;
		const vector<Contact> &frame = contactFrame.frame;
		if(frame.size() < minFingers || (numFingers.size() > 0 && numFingers.find(frame.size()) == numFingers.end())) //Diff error message if any.
		{
			return result;
		}
		if(frame.size() < 2) // atleast two fingers required for this test
			return result;
		double dx = fabs(frame[0].x - frame[1].x);
		double dy = fabs(frame[0].y - frame[1].y);
		result.push_back(dx);
		result.push_back(dy);
		return result;
	}
};

/**
 * Angle parameter. Returns angle made by the two fingers with the positive X axis of index1
 */
class fing_angle_parameter : public gesture_parameter
{
public:
	fing_angle_parameter(set<int> _numFingers, unsigned int _minFingers, bool _delta, int finger_index1, int finger_index2)
	{
		delta = _delta;
		numFingers = _numFingers;
		minFingers = _minFingers;
		fingerIndeces.push_back(finger_index1);
		fingerIndeces.push_back(finger_index2);
	}

	virtual vector<double> operator()(ContactSetFrame & contactFrame)
	{
		vector<double> result;
		const vector<Contact> &frame = contactFrame.frame;
		if(frame.size() < minFingers || (numFingers.size() > 0 && numFingers.find(frame.size()) == numFingers.end())) //Diff error message if any.
		{
			return result;
		}
		if(frame.size() < 2) // atleast two fingers required for this test
			return result;

		double dx = frame[0].x - frame[1].x;
		double dy = frame[0].y - frame[1].y;

		result.push_back(atan(dy / dx) * 180 / 3.14159265);
		return result;
	}
};
/**
 * The x,y coordinates of the mean of all fingers on screen produced as two parameters
 */
class all_mean_parameter : public gesture_parameter
{
public:
	all_mean_parameter(set<int> _numFingers, unsigned int _minFingers, bool _delta)
	{
		delta = _delta;
		numFingers = _numFingers;
		minFingers = _minFingers;
	}

	virtual vector<double> operator()(ContactSetFrame & contactFrame)
	{
		vector<double> result;
		double meanX = 0;
		double meanY = 0;

		if(contactFrame.size() < minFingers || (numFingers.size() > 0 && numFingers.find(contactFrame.size()) == numFingers.end())) //Diff error message if any.
			return result;

		BOOST_FOREACH(Contact c, contactFrame.frame)
		{
			meanX += c.x;
			meanY += c.y;
		}
		meanX /= contactFrame.size();
		meanY /= contactFrame.size();
		result.push_back(meanX);
		result.push_back(meanY);
		return result;
	}
};

class param_filter
{
public:
	virtual bool accepts(const ContactSetFrame & frame)
	{
		return false;
	}

	virtual void clear(){}
};

class delta_dist_filter : public param_filter
{
public:
	delta_dist_filter(int fing_index1, int fing_index2)
	:fing_index1(fing_index1), fing_index2(fing_index2), prevDist(-1), prevX1(-1), prevX2(-1), prevY1(-1), prevY2(-1), cleared(true)
	{
	}

	virtual bool accepts(const ContactSetFrame & contactFrame)
	{
		bool result = false;
		const vector<Contact> &frame = contactFrame.frame;

		if(frame.size() < 2) // atleast two fingers required for this test
			return result;

		//One finger > threshold && other finger < threshold
		float dx1 = (frame[0].x - prevX1);
		float dx2 = (frame[1].x - prevX2);
		float dy1 = (frame[0].y - prevY1);
		float dy2 = (frame[1].y - prevY2);

		float deltaDistFing1 = abs(sqrt(dx1*dx1 + dy1*dy1));
		float deltaDistFing2 = abs(sqrt(dx2*dx2 + dy2*dy2));

		if(cleared || prevX1 == -1 || prevX2 == -1 || prevY1 == -1 || prevY2 == -1) // On first check or when one of the fingers is raised
			result = false;
		else if((deltaDistFing1 > 2 && deltaDistFing2 < 2) || (deltaDistFing1 < 2 && deltaDistFing2 > 2) || (deltaDistFing1 + deltaDistFing2 < 5))
			result = false;
		else if(!(deltaDistFing1 == 0 && deltaDistFing2 == 0)) // If both aren't moving, all_mean is insignificant
			result = true; // Both fingers have a greater delta distance, all_mean is now different and valuable

//		cout << "deltaDistFing1: " << deltaDistFing1 << "\tdeltaDistFing2: " << deltaDistFing2 << "\tAccept ? " << result << endl;

		float dx = (frame[0].x - frame[1].x);
		float dy = (frame[0].y - frame[1].y);

		float dist = sqrt(dx*dx + dy*dy);

		if(!cleared && result == true && prevDist != -1 ) //movement threshold in pixels
		{
			//cout << "Delta Dist < 2" << endl;
			if(abs(prevDist - dist) < 2.5)
				result = true;
			else
				result = false;
		}

		cout << "deltaDistFing1: " << deltaDistFing1 << "\tdeltaDistFing2: " << deltaDistFing2 <<  "\tCurrDistance: " << dist << "\tPrev: " << prevDist << "\tAccept ? " << result << endl;

		prevX1 = frame[0].x;
		prevX2 = frame[1].x;
		prevY1 = frame[0].y;
		prevY2 = frame[1].y;
		prevDist = dist;
		cleared = false;
		return result;
	}

	virtual void clear()
	{
		cleared = true;
		prevX1 = prevX2 = prevY1 = prevY2 = -1;
		prevDist = -1;
	}
private:
	int fing_index1, fing_index2;
	double prevDist;
	double prevX1, prevX2, prevY1, prevY2;
	bool cleared;
};
/**
 * Instances of this class will allow a vector to be translated into
 * a parameter
 * fing_dist 0 1 	| will generate a parameter providing distance between two fingers
 * fing_x 0 		| will make x coord of finger 0 a parameter
 * fing_y 1			| will make y coord of finger 1 a parameter
 * fing_angle 0 1	| will calculate angle made by line from 0 to 1 with the positive x.
 * all_mean			| produces 2 parameters, the x,y coords of the mean of all points on screen.
 */
class gesture_parameterization
{
public:
	typedef pair<string, string> namedPairT;
	typedef pair<string, vector<double> > namedParamValPairT;
	typedef map<string, vector<double> > namedParamValMapT;
	typedef vector<namedParamValPairT> namedParamVectorT;
//	typedef pair<string, gesture_parameter> paramPairT;
	typedef boost::ptr_map<string, gesture_parameter> namedParamMapT;
	typedef boost::ptr_map<string, param_filter> namedFilterMapT;

	namedParamMapT 		namedParamsMap;
	namedParamValMapT 	prevVals;
	namedFilterMapT		namedFilterMap;

	gesture_parameterization()
	{}

	gesture_parameterization(namedPairT namedPair)
	{
		addParameter(namedPair);
	}


	void addParameter(namedPairT namedPair)
	{
		string paramName = namedPair.first;
		string paramString = namedPair.second;

		//Note: Boost tokenizer uses _ as a token seperator by default ! workaround this...
		boost::char_separator<char> sep(" ");
		boost::tokenizer<boost::char_separator<char> > 				tok(paramString, sep);
		boost::tokenizer<boost::char_separator<char> >::iterator 	token=tok.begin();

		set<int> num_fingers; //does not check
		unsigned int min_fingers = 0; //By default, this check passes
		//Instantiate the appropriate parameter class and add to the params vector.
		if(*token == "min_fingers")
		{
			min_fingers = boost::lexical_cast<int>(*++token);
			token++;
		}
		else if(*token == "num_fingers")
		{
			token++;
			while(*token != "delta")
			{
				cout << paramName << " NumFingers: " << *token << endl;
				num_fingers.insert(boost::lexical_cast<int>(*token));
				token++;
			}

		}

		if(*token == "delta_dist_filter")
		{
			int fing_index1 = boost::lexical_cast<int>(*++token);
			int fing_index2 = boost::lexical_cast<int>(*++token);
			namedFilterMap.insert(paramName, new delta_dist_filter(fing_index1, fing_index2));
			token++;
		}


		bool delta = false;
		if(*token == "delta") //Caches previous value and sends difference only
		{
			delta = true;
			token++;
		}

		if(*token == "fing_dist")
		{
			int fing_index1 = boost::lexical_cast<int>(*++token);
			int fing_index2 = boost::lexical_cast<int>(*++token);
			namedParamsMap.insert(paramName , new fing_dist_parameter(num_fingers, min_fingers, delta, fing_index1,fing_index2));
		}
		else if(*token == "fing_diffXY")
		{
			int fing_index1 = boost::lexical_cast<int>(*++token);
			int fing_index2 = boost::lexical_cast<int>(*++token);
			namedParamsMap.insert(paramName , new fing_diffXY_parameter(num_fingers, min_fingers, delta, fing_index1,fing_index2));
		}
		else if(*token == "fing_x")
		{
			int fing_index = boost::lexical_cast<int>(*++token);
			namedParamsMap.insert(paramName ,  new fing_x_parameter(num_fingers, min_fingers, delta, fing_index));
		}
		else if(*token == "fing_y")
		{
			int fing_index = boost::lexical_cast<int>(*++token);
			namedParamsMap.insert(paramName ,  new fing_y_parameter(num_fingers, min_fingers, delta, fing_index));
		}
		else if(*token == "fing_angle")
		{
			int fing_index1 = boost::lexical_cast<int>(*++token);
			int fing_index2 = boost::lexical_cast<int>(*++token);
			namedParamsMap.insert(paramName ,  new fing_angle_parameter(num_fingers, min_fingers, delta, fing_index1, fing_index2));
		}

		else if(*token == "all_mean")
		{
			namedParamsMap.insert(paramName , new all_mean_parameter(num_fingers, min_fingers, delta));
		}
		cout << "Instantiated gesture parameter: " << paramString << ". Gesture now has " << namedParamsMap.size() <<  " params" << endl;

	}

	/**
	 * Parameterization call
	 */
	namedParamValMapT operator()(ContactSetFrame & contactFrame)
	{
		namedParamValMapT resultMap;
		for( namedParamMapT::iterator param = namedParamsMap.begin(), e = namedParamsMap.end(); param != e; ++param )
		{
			string 				paramName 		= param->first;
			gesture_parameter*	parameter 		= param->second;
			namedFilterMapT::iterator iter 		= namedFilterMap.find(paramName);

			if(iter != namedFilterMap.end())
			{
				//cout << "Seeking approval from filter for param: " << paramName << endl;
				if(!iter->second->accepts(contactFrame))
					continue;								//Skip this param, because filter has not accepted this frame
			}
			vector<double> 		currParamValues = parameter->operator()(contactFrame);

			if(currParamValues.empty())
				continue;									//Skip further processing, cause no param values exist
			//equate near zero values to 0
			BOOST_FOREACH(double &currVal, currParamValues)
				if(fabs(currVal) <= 1e-8)
					currVal = 0;
			namedParamValPairT 	paramVals(paramName, currParamValues );

			if(parameter->delta)
			{
				//size_t prevValsSize = prevVals[paramName].size();
//				if(prevVals.empty() || prevValsSize == 0) //After a clear
//				{
//					cout << "Inserting paramVals:" << paramName << endl;
//					prevVals.insert(paramVals);
//					//paramVals.second must be set to zero
//					//paramVals.second.clear(); //No delta data this frame.
//				}
//				else
//				{
					vector<double> deltaVals;
					vector<double> prevParamValues = prevVals[paramName];
					if(prevParamValues.size() > 0)
					{
						for(size_t i = 0; i < prevParamValues.size(); i++) //prevParamValues and currParamValues should have same size.
						{
							double deltaVal = currParamValues[i] - prevParamValues[i];
//							if(abs(deltaVal) > 10)
//								cout << "Large deltaVal :(" << endl;
							deltaVals.push_back(deltaVal);
						}
						paramVals.second = deltaVals; //Set result to the difference in vals;
					}
					else
						paramVals.second.clear(); // no params to send
					prevVals[paramName] = currParamValues;
					//cout << "PrevVals for " << paramName << " size: " << prevVals[paramName].size() << endl;
//				}
			}
			if(paramVals.second.size() > 0)
				resultMap.insert(paramVals);
		}
		return resultMap;
	}

	/**
	 * Clear the cache of prevVals for delta calculations. Called on pause
	 */
	void clearPrevVals()
	{
//		cout << "Clear Vals" << endl;
		prevVals.clear();
//		cout << "PrevVals is now size: " << prevVals.size() << endl;
	}

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & namedParamsMap;
    }
};
#endif /* GESTUREPARAMETERIZATION_H_ */
